/*
 *  meanshift.cpp
 *  Woochy
 *
 *  Created by MiGi on 11/27/09.
 *  Copyright 2009 OSU. All rights reserved.
 *
 */

#include <math.h>
#include "meanshift.h"

int hdims = 16;   
float hranges_arr[] = {0,180};   // todo: move to cpp file
int vmin = 10, vmax = 256, smin = 30;   


MeanShift::MeanShift( CvSize size) {
    hranges = hranges_arr;
    // allocate images
    hsv = cvCreateImage( size, 8, 3 );   
    hue = cvCreateImage( size, 8, 1 );   
    mask = cvCreateImage( size, 8, 1 );       
    backproject = cvCreateImage( size, 8, 1 );   
    hist = cvCreateHist( 1, &hdims, CV_HIST_ARRAY, &hranges, 1 );   
    histimg = cvCreateImage( cvSize(320,200), 8, 3 );   
    cvZero( histimg );   
    
    initialized = false;
}

bool MeanShift::init(IplImage *image, const CvRect &selection) {
    int i, bin_w;  
    
    // init  
    int _vmin = vmin, _vmax = vmax;   
    cvCvtColor( image, hsv, CV_BGR2HSV );   
    cvSplit( hsv, hue, 0, 0, 0 );           
    cvInRangeS( hsv, cvScalar(0,smin,MIN(_vmin,_vmax),0),   
               cvScalar(180,256,MAX(_vmin,_vmax),0), mask );   
    
    
    float max_val = 0.f;   
    cvSetImageROI( hue, selection );   
    cvSetImageROI( mask, selection );   
        cvCalcHist( &hue, hist, 0, 0 );  // cvCalcHist( &hue, hist, 0, mask );   
    cvGetMinMaxHistValue( hist, 0, &max_val, 0, 0 );   
    cvConvertScale( hist->bins, hist->bins, max_val>0 ? 255. / max_val : 0., 0 );   
    cvResetImageROI( hue );   
    cvResetImageROI( mask );   
    track_window = selection;       
    
    // show histimg
    cvZero( histimg );   
    bin_w = histimg->width / hdims;   
    for( i = 0; i < hdims; i++ )   
    {   
        int val = cvRound( cvGetReal1D(hist->bins,i)*histimg->height/255 );   
        CvScalar color = hsv2rgb(i*180.f/hdims);   
        cvRectangle( histimg, cvPoint(i*bin_w,histimg->height),   
                    cvPoint((i+1)*bin_w,histimg->height - val),   
                    color, -1, 8, 0 );   
    }   
    cvNamedWindow( "Histogram", 1 );   
    cvShowImage( "Histogram", histimg );   
    
    initialized = true;
    return true;
}    

bool MeanShift::run(IplImage *image) 
{  
    if (!initialized) return false;
    cvCvtColor( image, hsv, CV_BGR2HSV );   
    int _vmin = vmin, _vmax = vmax;   
    
    //cvInRangeS( hsv, cvScalar(0,smin,MIN(_vmin,_vmax),0),   
    //           cvScalar(180,256,MAX(_vmin,_vmax),0), mask );   
    cvSplit( hsv, hue, 0, 0, 0 );       
        
    // backproject        
    cvCalcBackProject( &hue, backproject, hist );   
    //cvAnd( backproject, mask, backproject, 0 );   
    
    int iters;
    iters = cvMeanShift( backproject, track_window,   
                cvTermCriteria( CV_TERMCRIT_ITER | CV_TERMCRIT_EPS , 20, 1 ),    
                &comp ); 
    printf("%d\n", iters);
    
    track_window= comp.rect;   
    CvPoint pt1,pt2;   
    pt1.x=comp.rect.x;   
    pt1.y=comp.rect.y;   
    pt2.x=comp.rect.x+comp.rect.width;   
    pt2.y=(comp.rect.y+comp.rect.height);   
    cvRectangle(image,pt1,pt2,CV_RGB(0,255,0));   
    
    //cvCvtColor( backproject, image, CV_GRAY2BGR ); 

    
    
    return true;
    
}
